#include "exPathBuilder.hpp"
#include "godot_cpp/variant/utility_functions.hpp"

#include <unordered_map>
#include <glm/gtx/norm.hpp>

#include <godot_cpp/classes/collision_shape3d.hpp>
#include <godot_cpp/classes/physics_direct_space_state3d.hpp>

glm::vec3 L_Vector3ToGLMVec3( Vector3 v )
{ return glm::vec3(v.x, v.y, v.z); }

bool L_AStarLeafFunc( const SpaceNode* c )
{ return (c->mask & FLAG_LEAF) != 0; }

//Value lower is consider closer to solution.
float L_AStarHeuristicFunc(SpaceNode* c, glm::vec3 e)
{   
    float res = glm::distance2(c->voxel.center, e);
    glm::vec3 corner;
    for(uint32_t i = 0; i < 8; ++i)
    {
        for(uint32_t s = 0; s < 3; ++s )
        { corner[s] = (i | 1 << s) == 0 ? -c->voxel.size : c->voxel.size; }
        res = glm::min( res, glm::distance2(corner, e ));
    }
    return res;
}

bool L_BuilderTreePruneFunc( const BuilderNode* node )
{ return (node->mask & FLAG_VALIDVOXEL) == 0; }

SpaceNodeHandler::SpaceNodeHandler( PhysicsDirectSpaceState3D* world )
{ m_world = world; }

bool SpaceNodeHandler::ValidCond( const BuilderNode* vol )
{
    const Voxel& v = vol->voxel;
    PhysicsShapeQueryParameters3D query;
    BoxShape3D box;
    box.set_size( Vector3(v.size, v.size, v.size) );
    box.set_margin( 0.04f );
    
    Transform3D trans;
    trans.set_origin( Vector3( v.center.x, v.center.y, v.center.z ) );
    query.set_collision_mask( 1 << 0);
    query.set_shape(&box);
    query.set_transform( trans );
    return m_world->intersect_shape( &query ).is_empty();
}

void SpaceNodeHandler::SplitCallback( const bool cond, BuilderNode* vol ){}

void SpaceNodeHandler::LeafCallback( const bool cond, BuilderNode* vol ){}

void ExPathBuilder::_bind_methods()
{
    ClassDB::bind_method(D_METHOD( "Init", "world" ), &ExPathBuilder::Init );
    ClassDB::bind_method(D_METHOD( "BuildSpaceInfo", "volume", "minsize", "selectPt" ), &ExPathBuilder::BuildSpaceInfo );
    ClassDB::bind_method(D_METHOD( "ExportSpaceInfo", "path" ), &ExPathBuilder::ExportSpaceInfo );
    ClassDB::bind_method(D_METHOD( "ImportSpaceInfo", "path" ), &ExPathBuilder::ImportSpaceInfo );
    ClassDB::bind_method(D_METHOD( "FillPathAreas", "centers", "sizes" ), &ExPathBuilder::FillPathAreas );
    ClassDB::bind_method(D_METHOD( "FillConnectionsLines", "lines" ), &ExPathBuilder::FillConnectionsLines );
    ClassDB::bind_method(D_METHOD( "FetchVolumeInfo", "dic" ), &ExPathBuilder::FetchVolumeInfo );
    ClassDB::bind_method(D_METHOD( "FindPath", "sp", "ep", "safearea", "path" ), &ExPathBuilder::FindPath );
}

void ExPathBuilder::Init( PhysicsDirectSpaceState3D* world )
{ m_testHandler = new SpaceNodeHandler( world ); }

void ExPathBuilder::_enter_tree()
{
    constexpr uint16_t blockSizes[] = { 1031, 257, 160,  83, 72,   47,  23,  16,   11,   5,    2,    1 };
    constexpr uint32_t blockCnts[] =  { 1024, 16,  8172, 64, 1024, 128, 256, 8172, 2048, 8172, 2048, 1 };
    MemoryPool::inst()->InitMemoryPool( blockSizes, blockCnts, 12 );
}

void ExPathBuilder::BuildSpaceInfo( Vector3 volume, float minsize, Vector3 selectPt )
{
    if( m_testHandler == nullptr ){
        UtilityFunctions::printerr("Terrain tester handler is not set, skiping space info building...");
        return;
    }
    Vector3 pos = get_global_position();
    glm::vec3 c = glm::vec3(pos.x, pos.y, pos.z);
    glm::vec3 s = glm::vec3(volume.x, volume.y, volume.z);
    glm::vec3 sp = glm::vec3( selectPt.x, selectPt.y, selectPt.z);

    BuilderRoot root;
    CreateBuilderRoot( root, c, s, minsize, m_testHandler );
    PruneBuilderRoot( root, L_BuilderTreePruneFunc );

    SetupBuilderProxies( root );
    SetupBuilderConnections( root );
    SelectBuilderArea( root, sp );

    BuildSpaceRoot( root, m_space );
    DeleteBuilderRoot( root );
    UtilityFunctions::print("space info child size: ", m_space.childrenCnt );
    UtilityFunctions::print("center: ", pos.x, ", ", pos.y, ", ", pos.z );
    UtilityFunctions::print("total node cnt: ", m_space.nodeCnt );

    char mmsg[512];
    MemoryPool::inst()->FetchAllBucketInfo(mmsg, 512, 1);
    UtilityFunctions::print( "Unittest Dumping memory pool leaking summery" );
    UtilityFunctions::print( "==================================================================" );
    UtilityFunctions::print( mmsg );
}

void ExPathBuilder::ExportSpaceInfo( String fPath )
{
    try
    {
        ExportToFile( fPath.ascii(), m_space );
    } catch ( const MemoryBucketAllocError& err )
    {
        UtilityFunctions::printerr("[MemoryPool]", err.what() );
    }
}

void ExPathBuilder::ImportSpaceInfo( String fPath )
{
    try {
        DeleteSpaceRoot( m_space );
        ImportFromFile( fPath.ascii(), m_space );
        UtilityFunctions::print("Imported ", m_space.nodeCnt, " nodes.");
        UtilityFunctions::print("connection cnt: ", m_space.connectionCnt );
    } catch ( const MemoryBucketAllocError& err )
    {
        UtilityFunctions::printerr("[MemoryPool]", err.what() );
    }
}

void ExPathBuilder::FillPathAreas( Array centers, Array sizes )
{
    centers.clear();
    sizes.clear();
    for( uint32_t i = 0; i < m_space.nodeCnt; ++i )
    {
        SpaceNode* n = m_space.nodes + i;
        if( (n->mask & FLAG_LEAF) != 0 )
        {
            Voxel& v = n->voxel;
            const float s = v.size - 0.5f;
            centers.append( Vector3( v.center.x, v.center.y, v.center.z ) );
            sizes.append( Vector3( s, s, s ) );
        }
    }
}

void ExPathBuilder::FillConnectionsLines( Array line )
{
    line.clear();
    SpaceNode* nodes = m_space.nodes;
    for(uint32_t i = 0 ; i < m_space.nodeCnt; ++i)
    {
        SpaceNode* cur = nodes + i;
        if( cur->connectionCnt > 0 )
        {
            for( uint32_t j = 0; j < cur->connectionCnt; ++j )
            {
                SpaceNode* con = nodes + cur->connections[j];
                if( cur < con )
                { M_PushConnectionLine( cur->voxel, con->voxel, line ); }
            }
        }
    }
}

void ExPathBuilder::FetchVolumeInfo( Dictionary info )
{
    info.clear();
    info["min_size"] = m_space.size;
    info["volume"] = Vector3(
        m_space.dimesions[0] * m_space.size,
        m_space.dimesions[1] * m_space.size,
        m_space.dimesions[2] * m_space.size
    );
    Vector3 vec3;
    info["center"] = Vector3(
        m_space.center.x,
        m_space.center.y,
        m_space.center.z
    );
}

void ExPathBuilder::FindPath( Vector3 s, Vector3 e, float sa, Array waypoints)
{
    glm::vec3 sp = glm::vec3(s.x, s.y, s.z);
    glm::vec3 ep = glm::vec3(e.x, e.y, e.z);

    CircularDeque<uint32_t> wpNodes;
    FindPathNodes(m_space, sp, ep, wpNodes, L_AStarLeafFunc, L_AStarHeuristicFunc );
    UpdatePathNode(m_space, sp, wpNodes);
    glm::vec3 wps[16];
    uint32_t wpCnt = FindPathByNodes( m_space, wpNodes, sp, ep, glm::vec3(0.5f, 0.5f, 0.5f), wps, 16 );
    if( wpCnt == 0 )
    {
        UtilityFunctions::print("Failed to find path for given start-end pair, check both points are reachable.");
    } else {
        for( uint32_t i = 0; i < wpCnt; ++i )
        { waypoints.append( Vector3( wps[i].x, wps[i].y, wps[i].z )); }
    }
}

void ExPathBuilder::M_PushConnectionLine( Voxel& v0, Voxel& v1, Array& line )
{
    const float voxelDiff = (v1.size + v0.size) * 0.5f;
    const glm::vec3 diff = v0.size <= v1.size ? v1.center - v0.center : v0.center - v1.center;
    const glm::vec3 ref = v0.size <= v1.size ? v0.center : v1.center;
    const float refSize = glm::min( v0.size, v1.size ) * 0.5f;
    Vector3 p0 = Vector3( ref.x, ref.y, ref.z );
    Vector3 p1 = p0;
    if( glm::abs(glm::abs(diff.x) - voxelDiff) < 0.1f )
    {
        // On X axis.
        float dir = diff.x > 0.0f ? 1.0f : - 1.0f;
        p0.x = p0.x + dir * refSize * 0.75f; // ( refSize - refSize * 0.25f );
        p1.x = p0.x + dir * refSize * 0.5f;
    } else if( glm::abs(glm::abs(diff.y) - voxelDiff) < 0.1f )
    {
        // On Y axis.
        float dir = diff.y > 0.0f ? 1.0f : - 1.0f;
        p0.y = p0.y + dir * refSize * 0.75f; //( refSize - v0.size * 0.25f );
        p1.y = p0.y + dir * refSize * 0.5f;
    } else if( glm::abs(glm::abs(diff.z) - voxelDiff) < 0.1f )
    {
        // On Z axis.
        float dir = diff.z > 0.0f ? 1.0f : - 1.0f;
        p0.z = p0.z + dir * refSize * 0.75f; //( refSize - v0.size * 0.25f );
        p1.z = p0.z + dir * refSize * 0.5f;
    }
    line.append( p0 );
    line.append( p1 );
}
